#include "map/mapper.h"

#include <nav_msgs/OccupancyGrid.h>

#define MAP_SIZE 500
#define MAP_RESOLUTION 0.02f
#define HALF_MAP_SIZE_METERS (MAP_SIZE * MAP_RESOLUTION / 2.0f)

Mapper::Mapper() :
	occupancy_grid_(MAP_SIZE, MAP_SIZE, MAP_RESOLUTION),
	map_data(MAP_SIZE * MAP_SIZE)
{
	ros::NodeHandle nh;
	map_publisher_ = nh.advertise<nav_msgs::OccupancyGrid>("occupancy_grid", 1);
}

void Mapper::render()
{
	addOccupancyGrid(map_data);

	render(map_data);
}

void Mapper::addSensorRay(vec2 from, vec2 to, bool hit)
{
	from.y += HALF_MAP_SIZE_METERS;
	from.x += HALF_MAP_SIZE_METERS;
	to.x += HALF_MAP_SIZE_METERS;
	to.y += HALF_MAP_SIZE_METERS;

	occupancy_grid_.add(from, to, hit);
}

void Mapper::addSensorRay(vec2 origin, float direction, float mean, float std_dev, float height)
{
	origin.x += HALF_MAP_SIZE_METERS;
	origin.y += HALF_MAP_SIZE_METERS;

	occupancy_grid_.addSensorRay(origin, direction, mean, std_dev, height);
}

float Mapper::probabilityOfSensorRay(vec2 origin, float direction, float mean, float std_dev, float height)
{
	origin.x += HALF_MAP_SIZE_METERS;
	origin.y += HALF_MAP_SIZE_METERS;

	return occupancy_grid_.probabilityOfSensorRay(origin, direction, mean, std_dev, height);
}

void Mapper::addHitWithDistribution(vec2 location, float std_dev, float height)
{
	location.x += HALF_MAP_SIZE_METERS;
	location.y += HALF_MAP_SIZE_METERS;
	occupancy_grid_.addHitWithDistribution(location, std_dev, height);
}

void Mapper::subtractRobotArea(vec2 location, float std_dev, float height)
{
	location.x += HALF_MAP_SIZE_METERS;
	location.y += HALF_MAP_SIZE_METERS;

	occupancy_grid_.addHitWithDistribution(location, std_dev, height);
}

void Mapper::addOccupancyGrid(std::vector<int8_t>& map_data)
{
	int count = 0;
	for (int y = 0; y < MAP_SIZE; y++) {
		for (int x = 0; x < MAP_SIZE; x++) {
			float prob = occupancy_grid_.probabilityOccupied(x, y);
			int8_t val;
			if (prob < 0.30f) {
				val = 0;
			} else if (prob > 0.70f) {
				val = 100;
			} else {
				val = -1;
			}

			map_data[count++] = val;
		}
	}
}

void Mapper::render(std::vector<int8_t>& map_data)
{
	nav_msgs::OccupancyGrid occupancyGrid;

	occupancyGrid.header.frame_id = "/floor";
	occupancyGrid.header.stamp = ros::Time::now();

	occupancyGrid.info.map_load_time = ros::Time::now();
	occupancyGrid.info.resolution = MAP_RESOLUTION;
	occupancyGrid.info.width = MAP_SIZE;
	occupancyGrid.info.height = MAP_SIZE;

	occupancyGrid.info.origin.position.x = -HALF_MAP_SIZE_METERS;
	occupancyGrid.info.origin.position.y = -HALF_MAP_SIZE_METERS;
	occupancyGrid.info.origin.position.z = 0.001f;

	occupancyGrid.info.origin.orientation.x = 0.0f;
	occupancyGrid.info.origin.orientation.y = 0.0f;
	occupancyGrid.info.origin.orientation.z = 0.0f;
	occupancyGrid.info.origin.orientation.w = 1.0f;

	occupancyGrid.data = map_data;

	map_publisher_.publish(occupancyGrid);
}
